/********************************************************************
 *
	文件名：	image.c
	功能  ：	计算图像中线。
	作者  ：	赵志明
	日期  ：	2013/11/20
	版本  ：	V1.0
 *
 *******************************************************************/
 #include "image.h"
 #include "common.h"
 
/***********************************************************************
 *
 函数名		:	image_left_offset
 参数		:	image:图像二维数组行指针.
 输出		:	无
 操作结果	:	提取一行左边偏移值。
 *
 ************************************************************************/
 
int16_t  image_left_offset(u8 (*image)[COL] , u8 n)
{
        unsigned int temp =  0;
	int16_t left_edge =   0;
//	while(left_edge > 0)
//	{
//		if(*(*(image + n) + left_edge) <= threshold) temp++;
//                else temp = 0;
//                if(temp == 3) break;
//		left_edge--;
//	}
//	return(left_edge -COL/2 );
        while(left_edge < COL)
        {
          if(*(*(image + n) + left_edge) >= threshold) temp++;
               else temp = 0;
               if(temp == 3) break;
		left_edge++;  //从左向右搜索。
	}
      return(left_edge -COL/2 );     
}

/***********************************************************************
 *
 函数名		:	image_right_offset
 参数		:	image:图像二维数组行指针.
 输出		:	无
 操作结果	:	提取一行右边偏移值。
 *
 ************************************************************************/
 
int16_t image_right_offset(u8 (*image)[COL] , u8 n)
{
       unsigned int temp = 0;
	int16_t right_edge = COL;
//	while(right_edge < COL)
//          {
//		if( *(*(image + n) + right_edge) <= threshold) temp++;
//                else temp = 0;
//                if( temp == 3) break;
//		right_edge++;
//          }
//	return(right_edge - COL/2);
        
        while(right_edge > 0)
          {
		if( *(*(image + n) + right_edge) >= threshold) temp++;
                else temp = 0;
                if( temp == 3) break;
		right_edge--;           //从右向左搜索。
          }
	return(right_edge - COL/2);
        
        
        
}

/***********************************************************************
 *
 函数名		:	image_err
 参数		:	image:图像二维数组行指针 
 输出		:	无
 操作结果	:	提取图像平均偏差。
 *
 ************************************************************************/
//unsigned char loss_line = 0; //判断是否丢线，如果丢线方向控制保持上一次。
void image_err(cars_status car,u8 m , u8 n)
{
  float temp = 0;
  int i;
//  loss_line = 0;
  car->direction_err_old = car->direction_err_new;
  for(i = m;i<= n ;i++)
  {  
   if(edge_l[i] == 98 && edge_r[i] ==97)
   {
     if(i < (n-m)/2)
//      loss_line = 1; 
    break;
   }
    temp  += (edge_l[i] + edge_r[i]) / 2.0;
  }
  car->direction_err_new = temp/(i-m +1 ) ;
}


/************新图像算法**************************/
 

void image_err_s(u8 image[ROW][COL],cars_status car)
{
  unsigned int i = 0,j = 0;
  unsigned int temp = 0;                                 //标记连续点的个数，三个才判断是边界。
  float err;                                            //存取偏差和.
  //定义需要的标志。
  unsigned int start1 = 0,end1 = 0,start2 = 0,end2 = 0; //用于标记第一行图像白色边界，经观察图像白色边界一般不会大于两块，故设置两个起始结束标志。
  unsigned int frist_flag = 0;                          //用于标记第一行第一个白的边界是否被标记，如果标记，则下一个边界的信息存储于start2,end2.
  unsigned int color_flag = 0;                          //用于搜索点的标记，0代表搜索白点，1代表搜索黑点。如果搜到了白点则需要搜索黑点。一开始搜索白点。
  unsigned int search_mid;                             //图像中线的搜索中心。
  car->direction_err_old = car->direction_err_new;      //保存上次偏差，用于直立控制的微分，也可能用于丢线后的控制。
  
  //搜索图像第一行，寻找整个图像中线搜索位置.
  while(i < COL)            
  {
    if(!color_flag)        //找白边。 
    { 
      if(image[0][i] >= threshold) temp++;               //找到白点计数值加1.
      else temp = 0;                                   // 判断是否连续，不连续，计数值清空。  
      if(temp == 3)                                     // 判断计数是否有三个连续黑点。
        {
          if(!frist_flag)                                 //标记是否是第一个白边。
           { start1 = i; color_flag = 1;}                   //如果是第一个白边，记录白边的位置.标记颜色,下一次找黑边。
          else
            { start2 = i; color_flag = 1;}
          temp = 0;
        }
    }
    else                        //找黑边。
    {
      if(image[0][i] < threshold) temp++;
      else temp =0;
      if(temp ==3)
      {
        if(!frist_flag)
          {end1 = i;color_flag = 0;frist_flag = 1;}       //第一个黑边过后，寻找下一个白边，第一个白边找完，标志之。
        else
          {end2 = i;}
        temp = 0;
            
      }
    }
    
  }
 
/*******现在开始判断第一行搜索的情况。*********
   1，跑道只有一个白区。分为两块（白 --黑）或者三块（黑--白--黑）search_mid = (start1+end1)/2
  2, 跑到分为两个白区。如果显然start ！=0 ；end2 可能不为0 ，或者为0 ，如果如果end2 = 0,意味着end2实际为COL，然后比较宽度。
 *********************************************/ 
  if(start1 && end1 && !start2 && !end2)
     search_mid = (start1 + end1)/2;
  if(start1 && end1 && start2)
  {
    if(!end2)
        end2 = COL;
    search_mid = (end1 - start1) > (end2 -start2) ? (end1 +start1)/2 : (end2  +start2)/2 ;
    
  }
 
  
 //搜寻图像左偏差。
   j = search_mid;
  temp = 0;
  for(i = 0; i < ROW ; i++)
  {
    while(j > 0)
    {
      if(image[i][j] < threshold) temp++;
      else temp = 0;
      if(temp == 3)
      { edge_l[i] = (j - COL/2) ; temp =0; break ;}
    j--;
    } 
  }
  
 //搜寻图像右偏差。
  j = search_mid;
  temp = 0;
  for(i = 0; i < ROW ; i++)
  {
    while(j < COL)
    {
      if(image[i][j] < threshold) temp++;
      else temp = 0;
      if(temp == 3)
      { edge_l[i] = (j - COL/2) ; temp =0; break ;}
       j++;
    } 
  }
  
  
  for( i = 0; i < COL ;i++)
  {
     if(edge_l[i] == 98 && edge_r[i] ==97)
  { 
   break;
  }
    err  += (edge_l[i] + edge_r[i]) / 2.0;
  }
   car->direction_err_new = err/(COL-i +1 ) ;
  
  
  }


/***********人字弯检测*************/
int renzi_xian(u8 image[ROW][COL],u8 row)
{
    u8 i = 2;
    u8 j = 0;
    u8 heibai_flag;

    u8 jiance_count;

    if(image[row][0] > threshold && image[row][1] > threshold)
    {
        heibai_flag = 0; //下一步检测黑线
        jiance_count = 3;
    }
    else if(image[row][0] < threshold && image[row][1] < threshold)
    {
        heibai_flag = 1;    //下一步检测白线
        jiance_count = 4;
    }
    else
        return 2;

    for(j = 0;j<jiance_count;j++)
    {
        switch(heibai_flag)
        {
        case 0:
            for(;i<COL-2;i++)
            {
                if(image[row][i] < threshold && image[row][i+1] < threshold && image[row][i+2] < threshold)
                {
                    heibai_flag = 1;
//                    count++;
                    break;
                }
            }
            break;
        case 1:
            for(;i<COL-2;i++)
            {
                if(image[row][i] > threshold && image[row][i+1] > threshold && image[row][i+2] > threshold)
                {
                    heibai_flag = 0;
//                    count++;
                    break;
                }
            }
            break;
        }

        if(i == COL-2)
            break;
    }

    if(j==jiance_count-1)
        return 1;
    else
        return 0;
}
u8 renzi_flag = 0;
void renzi(u8 image[ROW][COL])
{
    int i=0;
    for(i = 0;i<ROW;i+=5)
    {
        if(renzi_xian(image,i) == 1)
        {
            renzi_flag = 1;
            break;
        }
    }
}
/*int renzi_xian(u8 image[ROW][COL],u8 row)
{
    u8 i = 2;
    u8 j = 0;
    u8 heibai_flag;

    u8 jiance_count;

    if(image[row][0] > threshold && image[row][1] > threshold)
    {
        heibai_flag = 0; //下一步检测黑线
        jiance_count = 3;
    }
    else if(image[row][0] < threshold && image[row][1] < threshold)
    {
        heibai_flag = 1;    //下一步检测白线
        jiance_count = 4;
    }
    else
        return 2;
    
    for(j = 0;j<jiance_count;j++)
    {
        switch(heibai_flag)
        {
        case 0:
            for(;i<COL;i++)
            {
                if(image[row][i] < threshold && image[row][i+1] < threshold)
                {
                    heibai_flag = 1;
//                    count++;
                    break;
                }
            }
            break;
        case 1:
            for(;i<COL;i++)
            {
                if(image[row][i] > threshold && image[row][i+1] > threshold)
                {
                    heibai_flag = 0;
//                    count++;
                    break;
                }
            }
            break;
        }
        
        if(i == COL)
            break;
    }
    
    if(j==jiance_count-1)
        return 1;
    else
        return 0;
}*/

int v_sharp_flag = 0;
int is_v_sharp(u8 (*image)[COL])
{
  int16_t start,end;
  start = image_left_offset(image  , 49) +COL/2;
  end   = image_right_offset(image ,49) +COL/2;
  if(start == 0 &&end != COL)
    return(-1);
  if(start !=0 && end == COL)
    return(1);
  return(0); 
 }
 

void image_s(cars_status car)
{
int i = 0,j;
unsigned int temp = 0;
float err = 0;
for(i = 0 ;i < ROW ; i++)
{
  edge_l[i] = 0;
  edge_r[i] = COL;
}
  
while(i < COL)
{
  if(image[0][i] > threshold) temp++;
  else temp = 0;
  if(temp == 3) {edge_l[0] = i - 3;break;}
  i++;
}
temp = 0; //计数值清0；
while(i < COL)
{
	if(image[0][i] <= threshold) temp++;
	else temp = 0;
	if(temp == 3) {edge_r[0] = i - 3;break;}
        i++;
}

err  =  (edge_l[0] + edge_r[0] - COL)/2.0;
for(i = 1 ; i < ROW ;i++)
{
	//从上一行的左边右移10点，向左搜索左边界。
	temp = 0;
	for(j = edge_l[i-1] + 10 ; j > 0;j--)  
	{       
                if(j>COL)
                  j = COL;
		if(image[i][j] < threshold) temp++;
		else temp = 0;
		if(temp == 3) {edge_l[i] = j - 3 ; break;}
	}
	temp = 0;
	for(j = edge_r[i-1] - 10; j > COL ; j++ )
	{
                if(j<0)
                  j = 0;
		if(image[i][j] > threshold) temp++;
		else temp = 0;
		if(temp == 3) {edge_r[i] = j - 3 ; break;}
	}
        err += (edge_l[i] + edge_r[i] - COL)/2.0;
}
car->direction_err_old = car->direction_err_new;
car->direction_err_new = err/ROW;
}


int is_v_sharp_s(u8 image[ROW][COL])
{
  int color_flag ;
  int i = 0;
  int temp;
  int white_flag1 = 0 , white_flag2 = 0;
  int frist_flag = 0;
  int flag1 = 0,flag2 = 0,flag3 =0;
  if(image[0][COL/4] > threshold)
  {color_flag = 1;white_flag1 = 1,frist_flag = 1;}                  //color_flag = 1 为白色边，0为黑边。
  else color_flag = 0;
     
  while(i < COL)            
  {
    if(!color_flag)        //找白边。 
    { 
      if(image[i][COL/4] >= threshold) temp++;               
      else temp = 0;                                    
      if(temp == 3)                                     
        {
          if(!frist_flag)                                 
           { white_flag1 = 1; color_flag = 1;frist_flag = 1;}                   
          else
            { white_flag2 = 1;break;}
          temp = 0;
        }
    }
    else                        //找黑边
    {
      if(image[i][COL/4] < threshold) temp++;
      else temp =0;
      if(temp ==3)
      {
        if(!frist_flag)
          {color_flag = 0;}  
        else
          {color_flag = 0;;break;}
        temp = 0;
            
      }
    }
  
  
  i++;
}
if(white_flag1 && white_flag2)
    flag1 = 1;
  else
    flag1 = 0;
  
 //清空标志。 
  color_flag = 0 ;
  i = 0;
  temp = 0;
  white_flag1 = 0 ;
  white_flag2 = 0;
  frist_flag = 0;
 
  if(image[0][COL/4] > threshold)
  {color_flag = 1;white_flag1 = 1,frist_flag = 1;}                  //color_flag = 1 为白色边，0为黑边。
  else color_flag = 0;
  while(i < COL)            
  {
    if(!color_flag)        //找白边。 
    { 
      if(image[i][COL/2] >= threshold) temp++;               
      else temp = 0;                                    
      if(temp == 3)                                     
        {
          if(!frist_flag)                                 
           { white_flag1 = 1; color_flag = 1;frist_flag = 1;}                   
          else
            { white_flag2 = 1;break;}
          temp = 0;
        }
    }
    else                        //找黑边
    {
      if(image[i][COL/2] < threshold) temp++;
      else temp =0;
      if(temp ==3)
      {
        if(!frist_flag)
          {color_flag = 0;}  
        else
          {color_flag = 0;;break;}
        temp = 0;
            
      }
    }
  
  
  i++;
}
  
  if(white_flag1 && white_flag2)
    flag2 = 1;
  else
    flag2 = 0;
//清空标志。
color_flag = 0 ;
i = 0;
temp = 0;
 white_flag1 = 0 ;
 white_flag2 = 0;
 frist_flag = 0;
 
  if(image[0][COL/4] > threshold)
  {color_flag = 1;white_flag1 = 1,frist_flag = 1;}                  //color_flag = 1 为白色边，0为黑边。
  else color_flag = 0;
 while(i < COL)            
  {
    if(!color_flag)        //找白边。 
    { 
      if(image[i][3*COL/4] >= threshold) temp++;               
      else temp = 0;                                    
      if(temp == 3)                                     
        {
          if(!frist_flag)                                 
           { white_flag1 = 1; color_flag = 1;frist_flag = 1;}                   
          else
            { white_flag2 = 1;break;}
          temp = 0;
        }
    }
    else                        //找黑边
    {
      if(image[i][3*COL/4] < threshold) temp++;
      else temp =0;
      if(temp ==3)
      {
        if(!frist_flag)
          {color_flag = 0;}  
        else
          {color_flag = 0;;break;}
        temp = 0;
            
      }
    }
  
  
  i++;
} 
  
  if(white_flag1 && white_flag2)
    flag3 = 1;
  else
    flag3 = 0;

  
  if(flag1 || flag2 || flag3)
    return(1);
  else
    return(0);
  
  
}






void image_f(u8 image[ROW][COL],cars_status car)
{
int i = 0,j;
unsigned int temp = 0;
int counter = 0;
float err = 0;
for(i = 0 ;i < ROW ; i++)
{
  edge_l[i] = 0;
  edge_r[i] = COL;
}
i = 0;
 while(i < COL)
 {
   if(image[0][i] > threshold) temp++;
   else temp = 0;
   if(temp == 3) {edge_l[0] = i - 2;break;}
   i++;
 }
 temp = 0; 
 while(i < COL)
 {
	 if(image[0][i] <= threshold) temp++;
	 else temp = 0;
	 if(temp == 3) {edge_r[0] = i - 3;break;}
         i++;
 }

err  =  (edge_l[0] + edge_r[0] - COL)/2.0;
for(i = 1 ; i < ROW ;i++)
{
	temp = 0;
	for(j = edge_l[i-1] + 10 ; j > 0;j--)
	{
                if(j>COL)
                  j = COL;
                if(image[i][j] <= threshold) 
		if(image[i][j] <= threshold) temp++;
		else temp = 0;
		if(temp == 3) {edge_l[i] = j +3 ; break;}
	}
	temp = 0;
	for(j = edge_r[i-1] - 10; j < COL ; j++ )
	{
                if(j<0)
                  j = 0;
		if(image[i][j] <= threshold) temp++;
		else temp = 0;
		if(temp == 3) {edge_r[i] = j - 3 ; break;}
	}
       if(edge_l[i] > edge_r[i])
       {     
         edge_l[i] =0;
         edge_r[i] =COL;
         counter++;
       }
        err += (edge_l[i] + edge_r[i] - COL)/2.0;
}
 car->direction_err_old = car->direction_err_new;
 car->direction_err_new = err/(COL - counter);
    
}


